﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Net.Sockets;
using System.Net;
using System.Text;
using System.Threading.Tasks;

namespace AntMovement.Core.Domain
{
    public class TPSocket
    {
        private Socket _socket;
        public TPSocket(string ip)
        {
            _socket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
            _socket.Bind(new IPEndPoint(IPAddress.Any, 22345));
            _socket.Connect(new IPEndPoint(IPAddress.Parse(ip), 8500));
            _socket.ReceiveBufferSize = 1024;
            _socket.SendBufferSize = 512;
            _socket.ReceiveTimeout = 1000;
        }

        public bool Heartbeat()
        {
            List<byte> data = new List<byte>();
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes(GetJSTimeUtc(DateTime.Now).ToString()));
            data.Add(0x23);
            List<byte> bytes = new List<byte>();
            bytes.AddRange(Encoding.ASCII.GetBytes("HRT"));
            bytes.Add((byte)data.Count);
            bytes.Add((byte)(data.Count >> 8));
            bytes.AddRange(data);
            _socket.Send(bytes.ToArray());
            byte[] buffer = new byte[128];
            int length = _socket.Receive(buffer);
            return length == 22;
        }

        /// <summary>
        /// 纠偏
        /// </summary>
        /// <param name="x_current">X当前坐标</param>
        /// <param name="y_current">Y当前坐标</param>
        /// <param name="r_current">R当前坐标</param>
        /// <param name="x_target">X目标坐标</param>
        /// <param name="y_target">Y目标坐标</param>
        /// <param name="r_target">R目标坐标</param>
        /// <returns></returns>
        public List<double> CorrectDeviation(float x_current, float y_current, float r_current, float x_target, float y_target, float r_target)
        {
            List<byte> data = new List<byte>();
            data.Add(0x23);
            data.Add(0);
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes(x_current.ToString("F2")));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes(y_current.ToString("F2")));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes(r_current.ToString("F2")));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes(x_target.ToString("F2")));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes(y_target.ToString("F2")));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes(r_target.ToString("F2")));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes(GetJSTimeUtc(DateTime.Now).ToString()));
            data.Add(0x23);
            List<byte> bytes = new List<byte>();
            bytes.AddRange(Encoding.ASCII.GetBytes("PGS"));
            bytes.Add((byte)data.Count);
            bytes.Add((byte)(data.Count >> 8));
            bytes.AddRange(data);
            _socket.Send(bytes.ToArray());
            byte[] buffer = new byte[256];
            _socket.ReceiveTimeout = 10000;
            int length = _socket.Receive(buffer);
            string str = Encoding.ASCII.GetString(buffer.Take(length).ToArray());
            string[] strings = str.Split('#');
            List<double> doubles = new List<double>();
            doubles.Add(buffer[6]);
            doubles.Add(buffer[8]);
            foreach (var item in strings)
            {
                if (double.TryParse(item, out double value))
                {
                    doubles.Add(value);
                }
            }
            return doubles;
        }

        /// <summary>
        /// 标定
        /// </summary>
        /// <param name="x_current">X当前坐标</param>
        /// <param name="y_current">Y当前坐标</param>
        /// <param name="r_current">R当前坐标</param>
        /// <param name="position">位置</param>
        /// <returns></returns>
        public List<double> Calibration(byte cameraNumber, float x_current, float y_current, float r_current, byte position)
        {
            List<byte> data = new List<byte>();
            data.Add(0x23);
            data.Add(cameraNumber);
            data.Add(0x23);
            data.Add(position);
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes(x_current.ToString("F2")));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes(y_current.ToString("F2")));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes(r_current.ToString("F2")));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes(GetJSTimeUtc(DateTime.Now).ToString()));
            data.Add(0x23);
            List<byte> bytes = new List<byte>();
            bytes.AddRange(Encoding.ASCII.GetBytes("CBS"));
            bytes.Add((byte)data.Count);
            bytes.Add((byte)(data.Count >> 8));
            bytes.AddRange(data);
            _socket.Send(bytes.ToArray());
            byte[] buffer = new byte[256];
            _socket.ReceiveTimeout = 30000;
            int length = _socket.Receive(buffer);
            string str = Encoding.ASCII.GetString(buffer.Take(length).ToArray());
            string[] strings = str.Split('#');
            List<double> doubles = new List<double>();
            doubles.Add(buffer[6]);
            doubles.Add(buffer[8]);
            foreach (var item in strings)
            {
                if (double.TryParse(item, out double value))
                {
                    doubles.Add(value);
                }
            }
            /*
                参数1：相机编号
                参数2：结果（PS：1标定成功，2标定失败，3标定未完成）
                参数3：X坐标
                参数4：Y坐标
                参数5：R坐标
                参数6：时间戳
             */
            return doubles;
        }

        public List<double> CBA(byte cameraNumber, float[] x_current, float[] y_current, byte position)
        {
            List<byte> data = new List<byte>();
            data.Add(0x23);
            data.Add(cameraNumber);
            data.Add(0x23);
            data.Add(position);
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes("0"));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes("0"));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes("0"));
            data.Add(0x23);
            #region 九点
            for (int i = 0; i < x_current.Length; i++)
            {
                data.AddRange(Encoding.ASCII.GetBytes(x_current[i].ToString("F2")));
                data.Add(0x23);
                data.AddRange(Encoding.ASCII.GetBytes(y_current[i].ToString("F2")));
                data.Add(0x23);
            }
            //data.AddRange(Encoding.ASCII.GetBytes(x_current[]));
            //data.Add(0x23);
            //data.AddRange(Encoding.ASCII.GetBytes("39.51"));
            //data.Add(0x23);

            //data.AddRange(Encoding.ASCII.GetBytes("-40.97"));
            //data.Add(0x23);
            //data.AddRange(Encoding.ASCII.GetBytes("39.51"));
            //data.Add(0x23);

            //data.AddRange(Encoding.ASCII.GetBytes("-91.15"));
            //data.Add(0x23);
            //data.AddRange(Encoding.ASCII.GetBytes("38.7"));
            //data.Add(0x23);

            //data.AddRange(Encoding.ASCII.GetBytes("9.04"));
            //data.Add(0x23);
            //data.AddRange(Encoding.ASCII.GetBytes("99.39"));
            //data.Add(0x23);

            //data.AddRange(Encoding.ASCII.GetBytes("-41.19"));
            //data.Add(0x23);
            //data.AddRange(Encoding.ASCII.GetBytes("98.85"));
            //data.Add(0x23);

            //data.AddRange(Encoding.ASCII.GetBytes("-91.44"));
            //data.Add(0x23);
            //data.AddRange(Encoding.ASCII.GetBytes("98.85"));
            //data.Add(0x23);

            //data.AddRange(Encoding.ASCII.GetBytes("8.5"));
            //data.Add(0x23);
            //data.AddRange(Encoding.ASCII.GetBytes("159.45"));
            //data.Add(0x23);

            //data.AddRange(Encoding.ASCII.GetBytes("-42.17"));
            //data.Add(0x23);
            //data.AddRange(Encoding.ASCII.GetBytes("158.78"));
            //data.Add(0x23);

            //data.AddRange(Encoding.ASCII.GetBytes("-92.43"));
            //data.Add(0x23);
            //data.AddRange(Encoding.ASCII.GetBytes("158.43"));
            //data.Add(0x23);
            #endregion
            data.AddRange(Encoding.ASCII.GetBytes(GetJSTimeUtc(DateTime.Now).ToString()));
            data.Add(0x23);
            List<byte> bytes = new List<byte>();
            bytes.AddRange(Encoding.ASCII.GetBytes("CBS"));
            bytes.Add((byte)data.Count);
            bytes.Add((byte)(data.Count >> 8));
            bytes.AddRange(data);
            _socket.Send(bytes.ToArray());
            byte[] buffer = new byte[256];
            _socket.ReceiveTimeout = 3000;
            int length = _socket.Receive(buffer);
            string str = Encoding.ASCII.GetString(buffer.Take(length).ToArray());
            string[] strings = str.Split('#');
            List<double> doubles = new List<double>();
            doubles.Add(buffer[6]);
            doubles.Add(buffer[8]);
            foreach (var item in strings)
            {
                if (double.TryParse(item, out double value))
                {
                    doubles.Add(value);
                }
            }
            /*
                参数1：相机编号
                参数2：结果（PS：1标定成功，2标定失败，3标定未完成）
                参数3：X坐标
                参数4：Y坐标
                参数5：R坐标
                参数6：时间戳
             */
            return doubles;
        }

        /// <summary>
        /// 摆盘
        /// </summary>
        /// <param name="x_current">X当前坐标</param>
        /// <param name="y_current">Y当前坐标</param>
        /// <param name="r_current">R当前坐标</param>
        /// <param name="position">位置</param>
        /// <returns></returns>
        public List<double> PGE()
        {
            List<byte> data = new List<byte>();
            data.Add(0x23);
            data.Add(1);
            data.Add(0x23);
            data.Add(3);
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes("-44"));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes("95"));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes("0"));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes("1.64"));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes("95"));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes("0"));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes("46.3"));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes("95"));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes("0"));
            data.Add(0x23);
            data.AddRange(Encoding.ASCII.GetBytes(GetJSTimeUtc(DateTime.Now).ToString()));
            data.Add(0x23);
            List<byte> bytes = new List<byte>();
            bytes.AddRange(Encoding.ASCII.GetBytes("PGS"));
            bytes.Add((byte)data.Count);
            bytes.Add((byte)(data.Count >> 8));
            bytes.AddRange(data);
            _socket.Send(bytes.ToArray());
            byte[] buffer = new byte[1280];
            _socket.ReceiveTimeout = 15000;
            int length = _socket.Receive(buffer);
            string str = Encoding.ASCII.GetString(buffer.Take(length).ToArray());
            string[] strings = str.Split('#');
            List<double> doubles = new List<double>();
            doubles.Add(buffer[6]);
            doubles.Add(buffer[8]);
            doubles.Add(buffer[10]);
            foreach (var item in strings)
            {
                if (double.TryParse(item, out double value))
                {
                    doubles.Add(value);
                }
            }
            /*
                参数1：相机编号
                参数2：结果（PS：1标定成功，2标定失败，4无料）
                参数3：X坐标
                参数4：Y坐标
                参数5：R坐标
                参数6：时间戳
             */
            return doubles;
        }

        static long GetJSTimeUtc(DateTime dateTime)
        {
            DateTime startTime = new DateTime(1970, 1, 1).ToLocalTime();
            return (long)(dateTime - startTime).TotalMilliseconds;
        }

        public void Exit()
        {
            try
            {
                _socket.Close();
                _socket.Dispose();
            }
            catch (Exception)
            {
            }
        }
    }
}
